{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridFactorGraph"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridFactorGraph.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "`HybridFactorGraph` is a `gtsam.FactorGraph` specifically designed to hold `gtsam.Factor` types that can represent hybrid discrete-continuous systems. While it can store any factor type (like its base class), it provides convenience methods for working with hybrid systems.\n",
        "\n",
        "It serves as a base class for more specialized hybrid factor graphs:\n",
        "*   `gtsam.HybridGaussianFactorGraph`: Holds factors that are either Gaussian, discrete, or hybrid Gaussian (linear conditional Gaussians dependent on discrete modes). Result of linearizing a `HybridNonlinearFactorGraph`.\n",
        "*   `gtsam.HybridNonlinearFactorGraph`: Holds factors that can be nonlinear, discrete, or hybrid nonlinear (nonlinear functions dependent on discrete modes).\n",
        "\n",
        "This notebook focuses on the base `HybridFactorGraph` interface. See the derived classes for specific elimination and usage examples."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "import graphviz\n",
        "\n",
        "from gtsam import (\n",
        "    PriorFactorPose2,\n",
        "    BetweenFactorPose2,\n",
        "    Pose2,\n",
        "    DecisionTreeFactor,\n",
        "    HybridFactorGraph,\n",
        "    HybridGaussianFactor,\n",
        "    JacobianFactor,\n",
        ")\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization and Adding Factors"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "HybridFactorGraph size: 4\n",
            "\n",
            "Graph Contents: \n",
            "size: 4\n",
            "factor 0: PriorFactor on x0\n",
            "  prior mean:  (0, 0, 0)\n",
            "  noise model: unit (3) \n",
            "factor 1:  f[ (d0,2), (d1,2), ]\n",
            " Choice(d1) \n",
            " 0 Choice(d0) \n",
            " 0 0 Leaf  0.1\n",
            " 0 1 Leaf  0.2\n",
            " 1 Choice(d0) \n",
            " 1 0 Leaf  0.9\n",
            " 1 1 Leaf  0.8\n",
            "factor 2: \n",
            "Hybrid [x5; d2]{\n",
            " Choice(d2) \n",
            " 0 Leaf :\n",
            "  A[x5] = [\n",
            "\t2\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            " 1 Leaf :\n",
            "  A[x5] = [\n",
            "\t0.5\n",
            "]\n",
            "  b = [ 1 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n",
            "factor 3: \n",
            "  A[x6] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n"
          ]
        }
      ],
      "source": [
        "# Create an empty HybridFactorGraph\n",
        "hfg = HybridFactorGraph()\n",
        "\n",
        "# --- Create diverse factors ---\n",
        "# Nonlinear (can be added, but usage depends on derived graph type)\n",
        "prior_nl = PriorFactorPose2(X(0), Pose2(0,0,0), gtsam.noiseModel.Unit.Create(3))\n",
        "between_nl = BetweenFactorPose2(X(0), X(1), Pose2(1,0,0), gtsam.noiseModel.Unit.Create(3))\n",
        "\n",
        "# Discrete\n",
        "dk1 = (D(0), 2)\n",
        "dk2 = (D(1), 2)\n",
        "discrete_f = DecisionTreeFactor([dk1, dk2], \"0.1 0.9 0.2 0.8\")\n",
        "\n",
        "# Hybrid Gaussian\n",
        "dk_h = (D(2), 2) # Hybrid mode key\n",
        "gf0 = JacobianFactor(X(5), np.eye(1)*2, np.zeros(1), gtsam.noiseModel.Unit.Create(1))\n",
        "gf1 = JacobianFactor(X(5), np.eye(1)*0.5, np.ones(1), gtsam.noiseModel.Unit.Create(1))\n",
        "hybrid_gf = HybridGaussianFactor(dk_h, [gf0, gf1])\n",
        "\n",
        "# Add factors to the graph\n",
        "hfg.push_back(prior_nl) # Index 0\n",
        "hfg.push_back(discrete_f) # Index 1\n",
        "hfg.push_back(hybrid_gf)  # Index 2\n",
        "# Can also add standard Gaussian factors if needed\n",
        "gaussian_f = JacobianFactor(X(6), np.eye(1), np.zeros(1), gtsam.noiseModel.Unit.Create(1))\n",
        "hfg.add(gaussian_f) # Index 3\n",
        "\n",
        "print(f\"HybridFactorGraph size: {hfg.size()}\")\n",
        "hfg.print(\"\\nGraph Contents:\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "fceac58c",
      "metadata": {},
      "outputs": [
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"360pt\" height=\"71pt\"\n",
              " viewBox=\"0.00 0.00 360.00 71.32\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(0.853081 0.853081) rotate(0) translate(4 79.6)\">\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 418,-79.6 418,4 -4,4\"/>\n",
              "<!-- var7205759403792793600 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var7205759403792793600</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">d0</text>\n",
              "</g>\n",
              "<!-- factor1 -->\n",
              "<g id=\"node8\" class=\"node\">\n",
              "<title>factor1</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"63\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793600&#45;&#45;factor1 -->\n",
              "<g id=\"edge2\" class=\"edge\">\n",
              "<title>var7205759403792793600&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M37.61,-40.75C46.9,-26.86 59.37,-8.22 62.34,-3.78\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793601 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var7205759403792793601</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">d1</text>\n",
              "</g>\n",
              "<!-- var7205759403792793601&#45;&#45;factor1 -->\n",
              "<g id=\"edge3\" class=\"edge\">\n",
              "<title>var7205759403792793601&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M88.39,-40.75C79.1,-26.86 66.63,-8.22 63.66,-3.78\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793602 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>var7205759403792793602</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">d2</text>\n",
              "</g>\n",
              "<!-- factor2 -->\n",
              "<g id=\"node9\" class=\"node\">\n",
              "<title>factor2</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"207\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793602&#45;&#45;factor2 -->\n",
              "<g id=\"edge5\" class=\"edge\">\n",
              "<title>var7205759403792793602&#45;&#45;factor2</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M181.61,-40.75C190.9,-26.86 203.37,-8.22 206.34,-3.78\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>var8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"315\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"315\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "<!-- factor0 -->\n",
              "<g id=\"node7\" class=\"node\">\n",
              "<title>factor0</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"315\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M315,-39.28C315,-25.77 315,-8.54 315,-3.96\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352325 -->\n",
              "<g id=\"node5\" class=\"node\">\n",
              "<title>var8646911284551352325</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"243\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"243\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x5</text>\n",
              "</g>\n",
              "<!-- var8646911284551352325&#45;&#45;factor2 -->\n",
              "<g id=\"edge4\" class=\"edge\">\n",
              "<title>var8646911284551352325&#45;&#45;factor2</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M232.39,-40.75C223.1,-26.86 210.63,-8.22 207.66,-3.78\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352326 -->\n",
              "<g id=\"node6\" class=\"node\">\n",
              "<title>var8646911284551352326</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"387\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"387\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x6</text>\n",
              "</g>\n",
              "<!-- factor3 -->\n",
              "<g id=\"node10\" class=\"node\">\n",
              "<title>factor3</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"387\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352326&#45;&#45;factor3 -->\n",
              "<g id=\"edge6\" class=\"edge\">\n",
              "<title>var8646911284551352326&#45;&#45;factor3</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M387,-39.28C387,-25.77 387,-8.54 387,-3.96\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x121a934a0>"
            ]
          },
          "execution_count": 4,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# We can visualize the graph using Graphviz\n",
        "graphviz.Source(hfg.dot())"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Inspecting Keys\n",
        "\n",
        "`HybridFactorGraph` provides methods to easily retrieve sets of discrete and continuous keys present in the graph."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Discrete KeySet: d0d1d2\n",
            "Continuous KeySet: x0x5x6\n",
            "All Keys (including nonlinear): d0d1d2x0x5x6\n"
          ]
        }
      ],
      "source": [
        "discrete_keyset = hfg.discreteKeySet() # Returns gtsam.KeySet\n",
        "print(f\"Discrete KeySet: {discrete_keyset}\")\n",
        "\n",
        "continuous_keyset = hfg.continuousKeySet() # Returns gtsam.KeySet\n",
        "print(f\"Continuous KeySet: {continuous_keyset}\")\n",
        "\n",
        "all_keys = hfg.keys() # Inherited from FactorGraph base class\n",
        "print(f\"All Keys (including nonlinear): {all_keys}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Error Calculation\n",
        "\n",
        "The `error` method sums the errors of all contained factors, assuming appropriate `Values` (or `HybridValues`) are provided. The exact interpretation depends on the factor types (nonlinear error, Gaussian negative log-likelihood, etc.)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Evaluating error with HybridValues:\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  x5: 0.1\n",
            "  x6: 0.2\n",
            "  Discrete: (d0, 0)(d1, 1)(d2, 0)\n",
            "  Nonlinear\n",
            "Values with 2 values:\n",
            "Value x0: (gtsam::Pose2)\n",
            "(0.1, 0, 0)\n",
            "\n",
            "Value x1: (gtsam::Pose2)\n",
            "(1.1, 0, 0)\n",
            "\n",
            "\n",
            "Total graph error: 0.15036051565782632\n"
          ]
        }
      ],
      "source": [
        "# Requires HybridValues to evaluate all factor types\n",
        "hybrid_values = gtsam.HybridValues()\n",
        "\n",
        "# Add Nonlinear values\n",
        "nl_values = gtsam.Values()\n",
        "nl_values.insert(X(0), Pose2(0.1, 0, 0)) # Small error for prior\n",
        "nl_values.insert(X(1), Pose2(1.1, 0, 0)) # Matches between factor\n",
        "hybrid_values.insert(nl_values)\n",
        "\n",
        "# Add Discrete values\n",
        "disc_values = gtsam.DiscreteValues()\n",
        "disc_values[D(0)] = 0\n",
        "disc_values[D(1)] = 1\n",
        "disc_values[D(2)] = 0 # Selects gf0 in the hybrid factor\n",
        "hybrid_values.insert(disc_values)\n",
        "\n",
        "# Add Continuous Vector values\n",
        "vec_values = gtsam.VectorValues()\n",
        "vec_values.insert(X(5), np.array([0.1])) # Value for hybrid_gf (component 0)\n",
        "vec_values.insert(X(6), np.array([0.2])) # Value for gaussian_f\n",
        "hybrid_values.insert(vec_values)\n",
        "\n",
        "print(\"\\nEvaluating error with HybridValues:\")\n",
        "hybrid_values.print()\n",
        "\n",
        "try:\n",
        "    total_error = hfg.error(hybrid_values)\n",
        "    print(f\"\\nTotal graph error: {total_error}\")\n",
        "    # Note: Error interpretation mixes nonlinear, discrete (-log(prob)), Gaussian\n",
        "except Exception as e:\n",
        "    print(f\"\\nError calculating graph error: {e}\")\n",
        "    print(\"Ensure HybridValues contains assignments for all keys in all factors.\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Further Operations\n",
        "\n",
        "Operations like elimination (`eliminateSequential`, `eliminateMultifrontal`) are typically performed on the more specific derived classes ([`HybridGaussianFactorGraph`](HybridGaussianFactorGraph.ipynb), [`HybridNonlinearFactorGraph`](HybridNonlinearFactorGraph.ipynb)). See their documentation for details."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
